#!/usr/bin/python3
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from yolo_pkg.msg import Yolo
from drone_pkg.msg import QRCode
import math
import tf
from drone_pkg.msg import SendString
from sensor_msgs.msg import Range
import serial
from drone_pkg.srv import *
import time
import os


def Laser_Init():
        os.system("gpio mode 10 out")
        os.system("gpio mode 13 out")
        os.system("gpio write 10 1")
        os.system("gpio write 13 1")

def Laser_Control(laser_id,state):
    if laser_id == 1 and state == 0:
        os.system("gpio write 10 1")
    if laser_id == 1 and state == 1:
        os.system("gpio write 10 0")
    if laser_id == 0 and state == 0:
        os.system("gpio write 13 1")
    if laser_id == 0 and state == 1:
        os.system("gpio write 13 0")



#控制类
class RobotController:
    def __init__(self):

        ####定义常量####
        self.MAX_LINEAR_SPEED =  36             #最大线速度，单位 cm/s
        self.MAX_ANGULAR_SPEED = 15             #最大角速度,单位 度/s

        self.KP_LINEAR_SPEED_POINT = 0.67        #坐标模式线速度KP
        self.KP_LINEAR_SPEED_DETECT = 0.055      #识别模式线速度KP
        self.KP_ANGULAR_SPEED = 0               #角速度环KP

        self.KP_Z_SPEED = 1
        self.MAX_Z_SPEED = 40

        self.MIN_ERROR_XY_POINT = 5             #坐标模式XY方向允许的最小误差，单位 cm
        self.MIN_ERROR_XY_DETECT = 33       #识别模式XY方向允许的最小误差，单位 像素
        self.MIN_ERROR_YAW = 25                 #YAW上最小角度误差，单位 度、
        
        ####定义变量####

        #目标变量
        self.targetPointX = 0                   #目标坐标：X，单位 cm
        self.targetPointY = 0                   #目标坐标：Y，单位 cm
        self.targetPointZ = 0                   #目标坐标：Z，单位 cm
        self.targetDetectX = 320                #识别模式X方向目标像素 单位 像素
        self.targetDetectY = 0                #识别模式Y方向目标像素 单位 像素
        self.targetDetectZ = 200                  #识别模式Z方向目标像素 单位 像素
        self.targetYaw = 0                      #目标角度YAW， 单位度
        self.targetNum = 8                      #目标数字
        self.targetHeight = 75                  #目标高度

        #中间变量
        self.distanceXY = 0                     #坐标模式距离目标点距离，单位 cm
        self.errorPointX = 0                    #坐标模式X方向偏差像素值，单位 cm
        self.errorPointY = 0                    #坐标模式Y方向偏差像素值，单位 cm
        self.errorPointZ = 0                    #坐标模式Z方向偏差像素值，单位 cm
        self.errorDetectX = 0                   #识别模式X方向偏差像素值，单位 像素
        self.errorDetectY = 0                   #识别模式Y方向偏差像素值，单位 像素
        self.errorDetectZ = 0                   #识别模式Z方向偏差像素值，单位 像素
        self.actualPositionX = 0                #实际位置x
        self.actualPositionY = 0                #实际位置y  
        self.actualPositionZ = 0                #实际位置z
        self.actualPositionYaw = 0              #实际位置YAW
        self.detectX = 0                        #识别到数字中心X坐标
        self.detectY = 0                        #识别到数字中心Y坐标
        self.detectZ = 0                        #识别到数字中心Z坐标
        self.errorYaw = 0                       #Yaw方向误差值，单位 度
        self.cosTheta = 0                       #目标坐标角度余弦值
        self.sinTheta = 0                       #目标坐标角度正弦值
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Delay = 0                #延时开启时间
        self.presentTime_Delay = 0              #延时当前时间
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Height = 0               #高度变换开启时间
        self.presentTime_Height = 0             #高度变换当前时间
        

        #结果变量
        self.resultLinearSpeedXY = 0            #计算输出XY合速度
        self.resultLinearSpeedX = 0             #计算输出X方向速度
        self.resultLinearSpeedY = 0             #计算输出Y方向速度
        self.resultLinearSpeedZ = 0             #计算输出Z方向速度
        self.resultAngularSpeedYaw = 0          #计算输出Yaw方向角速度
        


        #状态变量
        self.flagMovePoint = 0                  #坐标飞行标志
        self.flagdetect = 0                     #目标点对应的flag
        self.mode = 0                           #飞行模式
        self.detectNum = -1                     #识别到的数字
        self.flagLaser = 0                      #激光灯状态变量，0为关闭， 1为开启
        self.flagDelay = 0                      #延时的状态
        self.flagHeight = 0                     #高度变化的状态
        self.flagland = 0                       #降落状态
        self.flagfly = 0                        #起飞状态

        #二维码相关变量
        self.CamID = -1                         #相机编号
        self.QRCodeCount = 0                    #识别二维码计数
        self.QRNum = [-1]*24
        self.QR_Laser_flag = 0                  #激光+二维码状态
        self.Detect_Cam = 1                     #目标使用的相机编号
        self.qr_codes_array = []
        self.last_code = -1
        self.qr_timeOut = 10                    #超时时间
        self.findQrcode_State = 0               #二维码状态
        self.time_qrcode = 0                    #二维码时间
        self.QR_posx = 0
        self.QR_posy = 0
        self.QR_posz = 0

        self.Qr_array = [{"data": -1, "pos_x": 0, "pos_y": 0, "count": 0, "loc": ""} for _ in range(24)]



        self.Time_0 = 0
        self.Time = 0
        self.sleep_Time0 = 0
        self.sleep_Time = 120
       
        self.stringMax = ""
        self.stringMin = ""
        self.min_data = -1
        self.max_data = -1


        

        #初始化ROS节点
        rospy.init_node('control', anonymous=True)
        self.rate = rospy.Rate(50)
        rospy.on_shutdown(self.Shutdown)
        #初始化速度话题发布器
        self.vel_msg = Twist()
        self.pubSpeed = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        #初始化字符串控制话题发布器
        self.sendstr_msg = SendString()
        self.pubString = rospy.Publisher('/Laser', SendString, queue_size=10)
        # #初始化串口6
        self.ser_bluetooth = serial.Serial("/dev/ttyS6", 115200)
        self.ser_fukong = serial.Serial('/dev/ttyS3', 115200)
        #初始化TF缓存
        self.tf_buffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(self.tf_buffer)
        rospy.loginfo("Listening for TF data...")

        self.sub_QRCode = rospy.Subscriber("/QRCode_detect", QRCode, self.QRCode_Callback, queue_size=10)

        #订阅高度数据
        self.sub_height = rospy.Subscriber("/anoros_dt/ano_alt", Range, self.Height_callback, queue_size=10)
        #初始化服务节点
        self.time_server = rospy.Service("Start_Time", Start_time, self.Time_req)

        Laser_Init()
        

        with open('/home/orangepi/catkin_ws/src/drone_pkg/qr_code_data.txt', 'w') as file:
            pass  # 这样会清空文件内容




        


    def Time_req(self, req):
        if (req.state1 == 1):
            print("resive")
            self.Time_0 = rospy.get_time()
            resp = Start_timeResponse(1)
            return resp
        else:
            resp = Start_timeResponse(0)
            return resp
        

    def Servo_control(self, num=0):
        buf = '@' + str(num) + '\r\n'
        # self.ser_fukong.write(buf.encode())
        

    #计算坐标模式速度
    def calculate_point_speed(self):

        
        #计算距离
        self.distanceXY = math.sqrt(self.errorPointX * self.errorPointX + self.errorPointY * self.errorPointY)

        #计算合速度大小
        self.resultLinearSpeedXY = self.KP_LINEAR_SPEED_POINT * self.distanceXY 
        self.resultLinearSpeedXY = max(min(self.resultLinearSpeedXY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)

        #计算三角函数
        if self.distanceXY != 0:
            self.cosTheta = self.errorPointX / (self.distanceXY)
            self.sinTheta = self.errorPointY / (self.distanceXY)


        #计算输出速度
        self.resultLinearSpeedX = self.resultLinearSpeedXY * self.cosTheta
        self.resultLinearSpeedY = self.resultLinearSpeedXY * self.sinTheta
        self.resultAngularSpeedYaw = self.KP_ANGULAR_SPEED * self.errorYaw

        self.resultLinearSpeedZ = self.KP_Z_SPEED * self.errorPointZ
        
        #速度限幅
        self.resultAngularSpeedYaw = max(min(self.resultAngularSpeedYaw, self.MAX_ANGULAR_SPEED), -self.MAX_ANGULAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_Z_SPEED), -self.MAX_Z_SPEED)
        
    
    #计算识别模式速度
    def calculate_detect_speed(self, ID):
        #计算误差像素
        if (ID == 0):
            self.errorDetectX = -(self.targetDetectX - self.detectX)
        elif (ID == 1):
            self.errorDetectX = (self.targetDetectX - self.detectX)
        #self.errorDetectY = self.targetDetectY - self.detectY
        self.errorDetectZ = (self.targetDetectZ - self.detectZ)
        #计算识别后速度
        self.resultLinearSpeedX = self.errorDetectX * self.KP_LINEAR_SPEED_DETECT
        #self.resultLinearSpeedY = self.errorDetectY * self.KP_LINEAR_SPEED_DETECT
        self.resultLinearSpeedZ = self.errorDetectZ * self.KP_LINEAR_SPEED_DETECT
        #计算限速
        self.resultLinearSpeedX = max(min(self.resultLinearSpeedX, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        #self.resultLinearSpeedY = max(min(self.resultLinearSpeedY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
    
    def Height_callback(self, alt):
        self.actualPositionZ = alt.range * 100
        #rospy.loginfo("Height:%f", self.actualPositionZ)
        

    def QRCode_Callback(self, Qrcode):
       
        if Qrcode.CamID == self.Detect_Cam:
        #识别到二维码
            self.CamID = Qrcode.CamID
            self.findQrcode_State = 1
            self.detectX = Qrcode.x + Qrcode.Width // 2
            self.detectY = Qrcode.y + Qrcode.Height // 2
            self.detectNum = int(Qrcode.Data)
            self.time_qrcode = self.Time
            
    def QRCode_detect(self, camid, ID):
        if (self.CamID == camid):
            self.resultLinearSpeedX = 0
            self.resultLinearSpeedY = 0
            self.resultLinearSpeedZ = 0
            self.mode = -1
            if self.detectNum not in self.QRNum:
                try:
                    #transform = self.tf_buffer.lookup_transform('map', 'laser_frame', rospy.Time(0))
                    transform = self.tf_buffer.lookup_transform('odom', 'laser', rospy.Time(0))
                    # 提取位姿数据
                    translation = transform.transform.translation
                    rotation = transform.transform.rotation

                    # 四元数转换为欧拉角
                    _, _, yaw = tf.transformations.euler_from_quaternion(
                                    (rotation.x, rotation.y, rotation.z, rotation.w))
                    self.actualPositionYaw = yaw * 180 / math.pi

                    # 获取当前坐标（单位：cm）
                    self.actualPositionX = translation.x * 100
                    self.actualPositionY = translation.y * 100
                except:
                    self.mode = 0
                    return 0
                if camid == 0:
                    dx = (self.detectX - 320) * 0.24
                    dz = (self.detectY - 240) * 0.24
                    self.QR_posx = self.actualPositionX + dx + 5.5
                    self.QR_posz = self.actualPositionZ - dz
                elif camid == 1:
                    dx = (self.detectX - 320) * 0.24
                    dz = (self.detectY - 240) * 0.24
                    self.QR_posx = self.actualPositionX - dx - 5.5
                    self.QR_posz = self.actualPositionZ - dz
                self.QR_posy = self.actualPositionY

                if dx >= 30 and dz >= 30:
                    self.mode = 0
                    return 0
                

                self.QRNum[self.QRCodeCount] = self.detectNum 
                self.QRCodeCount += 1
                send_buf = f"@H{self.detectNum:02d}{ID}\r\n"
                print(send_buf)
                for i in range(15):
                    self.ser_bluetooth.write(send_buf.encode())
                # 打开文件并写入
                try:
                    with open("/home/orangepi/catkin_ws/src/drone_pkg/qr_code_data.txt", "a") as file:
                        # 将detectNum, xyz位置, ID, camid等信息写入文件
                        file.write(f"QRCodeCount: {self.QRCodeCount:02d}, detectNum: {self.detectNum:02d}, "
                                f"ID: {ID}, camid: {camid}, x: {int(self.QR_posx):05d}, y: {int(self.QR_posy):05d}, z: {int(self.QR_posz):05d}\n")
                except IOError as e:
                    rospy.logwarn(f"文件写入错误: {e}")
                    self.mode = 0
                    return 0


                
                return 1
        self.mode = 0
        return 0
 
    
    #超时检测
    def Qrcode_TimeOut(self):
        self.last_code = -1
        if self.findQrcode_State == 1:
            self.findQrcode_State = 0
            self.QRCodeCount += 1
            #self.send_blutooth(1)

            
    def send_blutooth(self, state):

        if state == 0:
            string = "@t" + str(int(self.Time * 10) / 10) + "\r\n" 
            self.ser.write(string.encode())
            print(string)
        else:
            string = "@" + str(self.QRCodeCount) + "T" + str(int(self.Time * 10) / 10) + "A" + str((self.detectNum)) + "D" + str(int((self.actualPositionX - 75) * 10) / 10) + "F" + str(int(self.Detect_Cam)) + "\r\n" 
            self.ser.write(string.encode())
            print(string)
            


    #坐标移动,更改目标坐标点
    #返回为真代表已经到达目的地
    def move(self, x, y, z, yaw=0, ignore_z = 0):
        self.mode = 0
        x_act, y_act, yaw_act = self.actualPositionX, self.actualPositionY, self.actualPositionYaw
        z_act = self.actualPositionZ
        self.targetPointX = x
        self.targetPointY = y
        self.targetPointZ = z
        self.targetYaw = yaw
        self.errorPointX = self.targetPointX - x_act   
        self.errorPointY = self.targetPointY - y_act
        self.errorPointZ = self.targetPointZ - z_act
        self.errorYaw = self.targetYaw - yaw_act
        if ignore_z == 0:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                    abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                    abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        else:
            return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT + 6 and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= 10)
    
            


    #判断是否到达目的地
    def judge_point(self):
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        
    #判断是否到达识别中心
    def judge_detect(self):
        return (abs(self.detectX - 320) <= self.MIN_ERROR_XY_DETECT and
                abs(self.detectY - 240) <= self.MIN_ERROR_XY_DETECT)
            
    def Delay(self, time):
        if self.flagDelay == 0:
            self.flagDelay = 1
            self.startTime_Delay = rospy.get_time()
            self.resultLinearSpeedX = 0
            self.resultLinearSpeedY = 0
            self.resultLinearSpeedZ = 0
            self.mode = -1
            return 0
        elif self.flagDelay == 1:
            self.presentTime_Delay = rospy.get_time()
            rospy.loginfo("WAITING")
            if (self.presentTime_Delay - self.startTime_Delay >= time):
                rospy.loginfo("Delay Over")
                self.flagDelay = 0
                self.mode = 0
                return 1
            else:
                return 0
            


    def Shutdown(self):
        rospy.loginfo("SHUT DOWN")
        self.vel_msg.linear.x = 0
        self.vel_msg.linear.y = 0
        self.vel_msg.linear.z = 0

        self.vel_msg.angular.z = 0
        self.pubSpeed.publish(self.vel_msg)

        self.Servo_control(0)

    def fly_up(self):
        for i in range(50):
            self.vel_msg.linear.z = 1
            self.pubSpeed.publish(self.vel_msg)
            rospy.loginfo("FLY%d", i)
        
    def land(self):
        for i in range(50):
            self.vel_msg.linear.z = -1
            self.pubSpeed.publish(self.vel_msg)
            #rospy.loginfo("FLY%d", i)




    #控制主程序
    def run(self):
        self.Detect_Cam = 0
        count = 0
        self.Time_0 = rospy.get_time()
        while not rospy.is_shutdown():
            count += 1
            if self.Time_0 != 0:
                self.Time = rospy.get_time() - self.Time_0
            else:
                self.Time = 0
            if (count >= 20):
                rospy.loginfo("flag:%d", self.flagMovePoint)
                rospy.loginfo("qrx:%.0f, y:%.0f, z:%.0f", self.QR_posx, self.QR_posy, self.QR_posz)
                count = 0
            
    
            
            try:
                #transform = self.tf_buffer.lookup_transform('map', 'laser_frame', rospy.Time(0))
                transform = self.tf_buffer.lookup_transform('odom', 'laser', rospy.Time(0))
                # 提取位姿数据
                translation = transform.transform.translation
                rotation = transform.transform.rotation

                # 四元数转换为欧拉角
                _, _, yaw = tf.transformations.euler_from_quaternion(
                                (rotation.x, rotation.y, rotation.z, rotation.w))
                self.actualPositionYaw = yaw * 180 / math.pi

                # 获取当前坐标（单位：cm）
                self.actualPositionX = translation.x * 100
                self.actualPositionY = translation.y * 100

                #具体控制写在这#
#############################################___CONTROL_CODE_BEGIN___###############################################################
                if self.flagMovePoint == 0 and self.move(0, 0, 132):
                    self.flagMovePoint = 1
                    self.Detect_Cam = 0
                elif self.flagMovePoint == 1 and self.move(0, -25, 132):
                    self.flagMovePoint = 2
            

                #A3
                elif self.flagMovePoint == 2 and self.move(-75, -25, 132):
                    if self.QRCode_detect(0, "A3"):
                        self.flagMovePoint = 3
                elif self.flagMovePoint == 3 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 4
                elif self.flagMovePoint == 4 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 5

                #A2
                elif self.flagMovePoint == 5 and self.move(-125, -25, 132):
                    if self.QRCode_detect(0, "A2"):
                        self.flagMovePoint = 6
                elif self.flagMovePoint == 6 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 7
                elif self.flagMovePoint == 7 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 8

                #A1
                elif self.flagMovePoint == 8 and self.move(-175, -25, 132):
                    if self.QRCode_detect(0, "A1"):
                        self.flagMovePoint = 9
                elif self.flagMovePoint == 9 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 10
                elif self.flagMovePoint == 10 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 11

                #A4
                elif self.flagMovePoint == 11 and self.move(-175, -25, 95):
                    if self.QRCode_detect(0, "A4"):
                        self.flagMovePoint = 12
                elif self.flagMovePoint == 12 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 13
                elif self.flagMovePoint == 13 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 14

                #A5
                elif self.flagMovePoint == 14 and self.move(-125, -25, 95):
                    if self.QRCode_detect(0, "A5"):
                        self.flagMovePoint = 15
                elif self.flagMovePoint == 15 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 16
                elif self.flagMovePoint == 16 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 17
                
                #A6
                elif self.flagMovePoint == 17 and self.move(-75, -25, 95):
                    if self.QRCode_detect(0, "A6"):
                        self.flagMovePoint = 18
                elif self.flagMovePoint == 18 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 19
                elif self.flagMovePoint == 19 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 20


                #下一面
                elif self.flagMovePoint == 20 and self.move(30, -25, 95):
                        self.flagMovePoint = 21
                elif self.flagMovePoint == 21 and self.move(30, 175, 95):
                        self.flagMovePoint = 22
                
                #B4
                elif self.flagMovePoint == 22 and self.move(-75, 175, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B4"):
                        self.flagMovePoint = 23
                elif self.flagMovePoint == 23 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 24
                elif self.flagMovePoint == 24 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 2401
        
                #C6
                elif self.flagMovePoint == 2401 and self.move(-75, 175, 95):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C6"):
                        self.flagMovePoint = 25
                elif self.flagMovePoint == 25 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 26
                elif self.flagMovePoint == 26 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 27


                #B5
                elif self.flagMovePoint == 27 and self.move(-125, 175, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B5"):
                        self.flagMovePoint = 28
                elif self.flagMovePoint == 28 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 29
                elif self.flagMovePoint == 29 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 2901
        
                #C5
                elif self.flagMovePoint == 2901 and self.move(-125, 175, 95):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C5"):
                        self.flagMovePoint = 30
                elif self.flagMovePoint == 30 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 31
                elif self.flagMovePoint == 31 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 32


                #B6
                elif self.flagMovePoint == 32 and self.move(-175, 175, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B6"):
                        self.flagMovePoint = 33
                elif self.flagMovePoint == 33 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 34
                elif self.flagMovePoint == 34 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 3401
        
                #C4
                elif self.flagMovePoint == 3401 and self.move(-175, 175, 95):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C4"):
                        self.flagMovePoint = 35
                elif self.flagMovePoint == 35 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 36
                elif self.flagMovePoint == 36 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 37
                    
                #B3
                elif self.flagMovePoint == 37 and self.move(-175, 175, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B3"):
                        self.flagMovePoint = 38
                elif self.flagMovePoint == 38 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 39
                elif self.flagMovePoint == 39 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 40
        
                #C1
                elif self.flagMovePoint == 40 and self.move(-175, 175, 132):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C1"):
                        self.flagMovePoint = 41
                elif self.flagMovePoint == 41 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 42
                elif self.flagMovePoint == 42 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 43

                #B2
                elif self.flagMovePoint == 43 and self.move(-125, 175, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B2"):
                        self.flagMovePoint = 44
                elif self.flagMovePoint == 44 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 45
                elif self.flagMovePoint == 45 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 46
        
                #C2
                elif self.flagMovePoint == 46 and self.move(-125, 175, 132):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C2"):
                        self.flagMovePoint = 47
                elif self.flagMovePoint == 47 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 48
                elif self.flagMovePoint == 48 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 49
                

                #B1
                elif self.flagMovePoint == 49 and self.move(-75, 175, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "B1"):
                        self.flagMovePoint = 50
                elif self.flagMovePoint == 50 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 51
                elif self.flagMovePoint == 51 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 52
        
                #C3
                elif self.flagMovePoint == 52 and self.move(-75, 175, 132):
                    self.Detect_Cam = 0
                    if self.QRCode_detect(0, "C3"):
                        self.flagMovePoint = 53
                elif self.flagMovePoint == 53 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(0, 1)
                    self.flagMovePoint = 54
                elif self.flagMovePoint == 54 and self.Delay(0.5):
                    Laser_Control(0, 0)
                    self.flagMovePoint = 55

                #下一面
                elif self.flagMovePoint == 55 and self.move(30, 175, 132):
                        self.flagMovePoint = 56
                elif self.flagMovePoint == 56 and self.move(30, 375, 132):
                        self.flagMovePoint = 57
                        self.Detect_Cam = 1
                
                #D1
                elif self.flagMovePoint == 57 and self.move(-75, 375, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D1"):
                        self.flagMovePoint = 58
                elif self.flagMovePoint == 58 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 59
                elif self.flagMovePoint == 59 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 60
                

                #D2
                elif self.flagMovePoint == 60 and self.move(-125, 375, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D2"):
                        self.flagMovePoint = 61
                elif self.flagMovePoint == 61 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 62
                elif self.flagMovePoint == 62 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 63
                

                #D3
                elif self.flagMovePoint == 63 and self.move(-175, 375, 132):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D3"):
                        self.flagMovePoint = 64
                elif self.flagMovePoint == 64 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 65
                elif self.flagMovePoint == 65 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 66

                
                #D6
                elif self.flagMovePoint == 66 and self.move(-175, 375, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D6"):
                        self.flagMovePoint = 67
                elif self.flagMovePoint == 67 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 68
                elif self.flagMovePoint == 68 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 69

                
                #D5
                elif self.flagMovePoint == 69 and self.move(-125, 375, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D5"):
                        self.flagMovePoint = 70
                elif self.flagMovePoint == 70 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 71
                elif self.flagMovePoint == 71 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 72


                
                #D4
                elif self.flagMovePoint == 72 and self.move(-75, 375, 95):
                    self.Detect_Cam = 1
                    if self.QRCode_detect(1, "D4"):
                        self.flagMovePoint = 73
                elif self.flagMovePoint == 73 and self.move(self.QR_posx, self.QR_posy, self.QR_posz) and self.judge_detect():
                    Laser_Control(1, 1)
                    self.flagMovePoint = 74
                elif self.flagMovePoint == 74 and self.Delay(0.5):
                    Laser_Control(1, 0)
                    self.flagMovePoint = 10000


                #终点

                elif self.flagMovePoint == 10000 and self.move(-245, 358, 95):
                    self.flagMovePoint = 10001
                elif self.flagMovePoint == 10001 and self.move(-245, 358, 4):
                    self.flagMovePoint = 10002
                    self.land()
                    self.land()
                    self.land()
                    self.land()
                    return
                    
                





#############################################___CONTROL_CODE_END___################################################################

                #计算速度
                if self.mode == 0:
                    self.calculate_point_speed()
                elif self.mode == 1:
                    self.calculate_detect_speed(self.Detect_Cam)

                    

                #发布速度,Twist中速度单位为m/s
                self.vel_msg.linear.x = self.resultLinearSpeedX * 0.01
                self.vel_msg.linear.y = self.resultLinearSpeedY * 0.01
                self.vel_msg.linear.z = self.resultLinearSpeedZ * 0.01


                self.vel_msg.angular.z = self.resultAngularSpeedYaw
                self.pubSpeed.publish(self.vel_msg)
                

                


            except tf2_ros.TransformException as e:
                rospy.logwarn("Transform not available: %s", e)
                pass
            self.rate.sleep()

#主函数
if __name__ == '__main__':
    try:
        controller = RobotController()
        controller.run()
    except rospy.ROSInterruptException:
        pass


